#!/usr/bin/python
# -*- coding: utf-8 -*-
import time
import numpy as np
from car_warp import MyCar
from tools import string_to_chunks
from infer_exe import Client


def release_block():
    my_car.adjust_pos_y(25, 26, timeout=10, speed=0.05)
    my_car.lane_distance(distance=0.06, axis=0, speed=0.1)
    my_car.arm.do_task("release_blue_block", wait=0.1)
    time.sleep(0.4)
    my_car.arm.do_task("release", wait=0.2)
    time.sleep(0.5)
    my_car.arm.do_task("release_red_block", wait=0.2)
    time.sleep(0.5)
    my_car.arm.do_task("release_red_block2", wait=0.1)
    time.sleep(0.4)
    my_car.arm.do_task("release", wait=0.2)
    my_car.arm.do_task("det_pos_left", wait=0.2)
    my_car.box.do_task("reset")
    my_car.lane_distance(distance=0.01, axis=1, speed=0.1)

    my_car.auto_lane_distance(distance=0.45, speed=0.15)
    my_car.coe = 0
    my_car.auto_lane_distance(distance=0.4, speed=0.15)
    my_car.coe = 1.2

    my_car.det_direct = -1
    return 6


def grab_ball():
    my_car.adjust_pos_x_by_sensor()
    my_car.box.do_task("store")
    my_car.adjust_pos_y(13, 13, speed=0.05, timeout=10)
    my_car.adjust_pos_omega(-0.0005, 0.003)
    my_car.adjust_pos_x(-8, 8)
    my_car.arm.do_task("grab_and_store_ball", wait=0.3)
    my_car.lane_distance(distance=0.09, axis=0, speed=0.15)
    my_car.adjust_pos_omega(-0.0005, 0.003)
    my_car.adjust_pos_x(-8, 8)
    my_car.arm.do_task("grab_and_store_ball", wait=0.3)
    my_car.lane_distance(distance=0.09, axis=0, speed=0.15)
    my_car.adjust_pos_x(-8, 8)
    my_car.arm.do_task("grab_and_store_ball", wait=0.3)
    my_car.rotor.do_task("start")


def elevate():
    my_car.adjust_pos_omega(-0.0001, 0.003)
    my_car.adjust_pos_x(-37, 2)
    my_car.adjust_pos_y(6, 7, timeout=7, speed=0.04)
    my_car.rotor.do_task("rote", wait=6.5)
    my_car.rotor.do_task("stop")
    my_car.lane_distance(distance=0.02, axis=1, speed=0.1)
    my_car.det_task.close()

    my_car.coe = 1.5
    my_car.auto_lane_distance(distance=0.6, speed=0.15)
    my_car.coe = 1.2


def grab_high_ball():
    my_car.adjust_pos_x_by_sensor()
    my_car.adjust_pos_y(11, 12, timeout=7, speed=0.04)
    my_car.lane_distance(distance=0.002, axis=0)
    my_car.adjust_pos_omega(-0.0001, 0.003)
    my_car.arm.do_task("grab_high_ball", wait=0.4)
    my_car.lane_distance(distance=0.003, axis=1, speed=0.1)


def pull():
    my_car.adjust_pos_y(9, 10)
    my_car.lane_distance(distance=0.08, axis=0, speed=0.1)
    my_car.box.do_task("pull")
    time.sleep(1)
    my_car.box.do_task("reset")
    my_car.lane_distance(distance=0.002, axis=1, speed=0.1)
    my_car.adjust_pos_omega(-0.002, 0.002)
    my_car.arm.do_task("det_pos_right")
    my_car.det_direct = 1


    my_car.coe = 1.5
    my_car.auto_lane_distance(distance=0.6, speed=0.15)
    my_car.coe = 1.2
    return 35


def hit():
    dis = {"01": 0.24, "02": 0.32, "03": 0.39, "04": 0.48}
    my_car.adjust_pos_omega(-0.0005, 0.001)
    my_car.det_task = Client("det_task")
    my_car.det_task.init()
    my_car.adjust_pos_x(-8, 8)
    # my_car.adjust_pos_x_by_sensor()
    my_car.adjust_pos_y(14, 14, timeout=5)
    my_car.adjust_pos_omega(-0.001, 0.001)
    # my_car.adjust_pos_x(-5, 5)
    my_car.lane_distance(distance=dis[my_car.which], axis=0, speed=0.07)
    # my_car.arm.do_task("hit", wait=1)
    my_car.adjust_pos_omega(-0.001, 0.001)
    my_car.lane_distance(distance=-0.01, axis=1, speed=0.1)
    my_car.box.do_task("horiz")
    my_car.lane_distance(distance=0.06, axis=1, speed=0.1)
    time.sleep(0.5)
    my_car.lane_distance(distance=-0.06, axis=1, speed=0.1)
    my_car.box.do_task("reset")
    my_car.task_monitor_over = True
    # my_car.lane_distance(distance=0.5, axis=0, speed=0.15)
    # my_car.end()


def get_which(dur=float("inf")):
    start = time.time()
    index = ['01', '02', '03', '04']
    command = "77 68 09 07 02 01 00 00 0A"
    command_bytes = bytes.fromhex(command)
    while True:
        if time.time() - start > dur:
            break
        ret, res = my_car.base_moto.ser.get_answer(command_bytes)
        if ret:
            res = string_to_chunks(res.hex(), 2)
            if len(res) == 9:
                if res[-3] != "00":
                    n = np.random.randint(0, 4)
                    return index[n]


if __name__ == "__main__":
    task_dict = {'house': release_block,
                 'blue_ball': grab_ball,
                 'pole': elevate,
                 'tower': grab_high_ball,
                 'pull': pull,
                 'board': hit
                 }
    my_car = MyCar(task_dict)  # 初始化智能车实例
    my_car.which = get_which()
    print(my_car.which)
    my_car.start()  # 小车开始运行
